/*
 * Copyright (c) 2006-2023, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2023-12-16     RT-Thread    first version
 * 2025-05-14     Trae AI      add thread management
 */

#include <rtthread.h>
#include <rtdevice.h>
#include "thread_manage.h"
#include "mpu6xxx.h"

/* 定义线程控制块 */
static struct rt_thread sensor_thread;
static struct rt_thread data_process_thread;

/* 定义线程栈 */
static rt_uint8_t sensor_thread_stack[SENSOR_THREAD_STACK_SIZE];
static rt_uint8_t data_process_thread_stack[DATA_PROCESS_THREAD_STACK_SIZE];

/* 定义消息队列控制块和消息队列池 */
static struct rt_messagequeue sensor_data_mq;
static rt_uint8_t sensor_data_mq_pool[SENSOR_DATA_MQ_SIZE * sizeof(struct sensor_data_msg)];

/* 定义事件控制块 */
static struct rt_event har_event;

/* 定义软件定时器控制块 */
static rt_timer_t data_report_timer;

/* 传感器数据采集线程入口函数 */
static void sensor_thread_entry(void *parameter)
{
    struct mpu6xxx_device *dev = (struct mpu6xxx_device *)parameter;
    struct sensor_data_msg msg;
    rt_err_t result;
    
    while (1)
    {
        /* 读取传感器数据 */
        mpu6xxx_get_accel(dev, &msg.accel);
        mpu6xxx_get_gyro(dev, &msg.gyro);
        
        /* 发送消息到消息队列 */
        result = rt_mq_send(&sensor_data_mq, &msg, sizeof(struct sensor_data_msg));
        
        /* 延时采样间隔 */
        rt_thread_mdelay(SAMPLE_INTERVAL);
    }
}

/* 数据处理线程入口函数 */
static void data_process_thread_entry(void *parameter)
{
    rt_device_t uart_dev = (rt_device_t)parameter;
    struct sensor_data_msg msg;
    rt_err_t result;
    rt_uint32_t sample_count = 0;
    char data_buffer[128];
    rt_size_t len;
    
    /* 等待系统稳定 */
    rt_thread_mdelay(1000);
    
    /* 发送启动消息 */
    rt_device_write(uart_dev, 0, "MPU6050 SENSOR DATA STREAM START\r\n", 34);
    rt_thread_mdelay(500); // 等待串口稳定
    
    while (1)
    {
        /* 发送数据窗口开始标记 */
        if (sample_count == 0)
        {
            rt_device_write(uart_dev, 0, "START_HAR_DATA\r\n", 16);
            rt_thread_mdelay(10); // 短暂延时确保标记被正确接收
        }
        
        /* 从消息队列接收消息 */
        result = rt_mq_recv(&sensor_data_mq, &msg, sizeof(struct sensor_data_msg), RT_WAITING_FOREVER);
        if (result == RT_EOK)
        {
            /* 将整数数据转换为浮点数格式 */
            float acc_x = msg.accel.x / 16384.0f;
            float acc_y = msg.accel.y / 16384.0f;
            float acc_z = msg.accel.z / 16384.0f;
            
            float gyro_x = msg.gyro.x / 131.0f;
            float gyro_y = msg.gyro.y / 131.0f;
            float gyro_z = msg.gyro.z / 131.0f;
            
            /* 格式化数据为浮点数格式 */
            len = rt_snprintf(data_buffer, sizeof(data_buffer), 
                             "%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\r\n", 
                             acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z);
            
            /* 通过串口发送数据 */
            rt_size_t send_size = rt_device_write(uart_dev, 0, data_buffer, len);
            if (send_size != len)
            {
                /* 发送事件通知错误处理 */
                rt_event_send(&har_event, EVENT_UART_ERROR);
            }
            
            /* 增加样本计数 */
            sample_count++;
            
            /* 当收集了足够的样本后，发送窗口结束标记 */
            if (sample_count >= WINDOW_SIZE)
            {
                rt_thread_mdelay(10); // 短暂延时确保数据被正确接收
                rt_device_write(uart_dev, 0, "END_OF_WINDOW\r\n", 15);
                sample_count = 0; // 重置样本计数
                rt_thread_mdelay(100); // 在窗口之间添加一些延迟
                
                /* 发送事件通知窗口完成 */
                rt_event_send(&har_event, EVENT_WINDOW_COMPLETE);
            }
        }
    }
}

/* 定时器超时回调函数 */
static void data_report_timer_callback(void *parameter)
{
    /* 发送事件通知需要生成报告 */
    rt_event_send(&har_event, EVENT_REPORT_NEEDED);
}

/* 初始化线程和通信机制 */
rt_err_t har_thread_init(rt_device_t uart_dev, struct mpu6xxx_device *sensor_dev)
{
    rt_err_t result;
    
    /* 初始化消息队列 */
    result = rt_mq_init(&sensor_data_mq, 
                        "sensor_mq", 
                        sensor_data_mq_pool, 
                        sizeof(struct sensor_data_msg), 
                        SENSOR_DATA_MQ_SIZE * sizeof(struct sensor_data_msg), 
                        RT_IPC_FLAG_FIFO);
    if (result != RT_EOK)
    {
        return result;
    }
    
    /* 初始化事件 */
    result = rt_event_init(&har_event, "har_event", RT_IPC_FLAG_FIFO);
    if (result != RT_EOK)
    {
        return result;
    }
    
    /* 创建软件定时器 */
    data_report_timer = rt_timer_create("report_timer", 
                                       data_report_timer_callback, 
                                       RT_NULL, 
                                       REPORT_TIMER_PERIOD, 
                                       RT_TIMER_FLAG_PERIODIC);
    if (data_report_timer == RT_NULL)
    {
        return -RT_ERROR;
    }
    
    /* 初始化传感器数据采集线程 */
    result = rt_thread_init(&sensor_thread, 
                           "sensor", 
                           sensor_thread_entry, 
                           sensor_dev, 
                           sensor_thread_stack, 
                           SENSOR_THREAD_STACK_SIZE, 
                           SENSOR_THREAD_PRIORITY, 
                           SENSOR_THREAD_TIMESLICE);
    if (result != RT_EOK)
    {
        return result;
    }
    
    /* 初始化数据处理线程 */
    result = rt_thread_init(&data_process_thread, 
                           "data_proc", 
                           data_process_thread_entry, 
                           uart_dev, 
                           data_process_thread_stack, 
                           DATA_PROCESS_THREAD_STACK_SIZE, 
                           DATA_PROCESS_THREAD_PRIORITY, 
                           DATA_PROCESS_THREAD_TIMESLICE);
    if (result != RT_EOK)
    {
        return result;
    }
    
    /* 启动线程 */
    rt_thread_startup(&sensor_thread);
    rt_thread_startup(&data_process_thread);
    
    /* 启动定时器 */
    rt_timer_start(data_report_timer);
    
    return RT_EOK;
}

/* 事件处理函数 */
void har_event_handler(void)
{
    rt_uint32_t recv_event;
    rt_err_t result;
    
    /* 等待事件 */
    result = rt_event_recv(&har_event, 
                          EVENT_UART_ERROR | EVENT_WINDOW_COMPLETE | EVENT_REPORT_NEEDED, 
                          RT_EVENT_FLAG_OR | RT_EVENT_FLAG_CLEAR, 
                          RT_WAITING_FOREVER, 
                          &recv_event);
}